﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;
using Drifting.MotionPlanning.PointSets;

namespace Drifting.MotionPlanning.SingleThreading.Algorithms
{
    public class RRT : TreeBasedPlanningAlgorithm
    {
        public RRT(BaseSystem system, Random rand)
            : base (system, rand)
        {
            int[] branches = new int[] { 6, 6, 6, 6, 6, 6, 6, 6 };   //{ 6, 6, 6, 6, 6, 6, 6, 6, 6 }
            PointSet Vactive = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);

            vertices =
                //new LinearQuery();
                //new AlphaRootQuery(rand, 1.5);
                Vactive;
            SetRoot(system.GenerateZeroState());
        }

        override public void Execute(object input)
        {
            if (input is Int32 == false)
                throw new ArgumentException();
            n_exec = (Int32)input;
            if (x_root == null)
                throw new InvalidOperationException("Forget to call SetRoot first?");

            double[] a_sample = null;
            n_current = 0;
            while (n_current < n_exec)
            {
                n_current++;

                //Sample
                system.SampleAlias(rand, ref a_sample);

                //Nearest neighbor search
                State x_nearest = null;
                double d2_nearest = double.MaxValue;
                vertices.Neighbor_SingleQuery(a_sample, ref x_nearest, ref d2_nearest);

                //Propagate
                double[] iCtrl = null;
                double iDt = double.MaxValue;
                system.SampleControl(rand, ref iCtrl, ref iDt);

                double[] q_new = null;
                double iCost = double.MaxValue;
                bool propagate_success = system.Propagate(memoryPad, x_nearest.Q, iCtrl, iDt, ref q_new, ref iCost);

                //Grow G
                if (propagate_success)
                {
                    State x_new = SpawnState(q_new);
                    Edge e_new = SpawnEdge(x_nearest, x_new, iCtrl, iDt, iCost);
                    x_new.ParentEdge = e_new;
                    x_new.Cost = x_nearest.Cost + iCost;

                    if (system.ValidateEdge(e_new) == false)
                        continue;

                    vertices.AddState(x_new);
                }

                //Auxillary
                //Thread.Sleep(3);
            }



        }

        override public void AliasOf(double[] q, ref double[] alias)
        {
            if (alias == null)
                alias = new double[system.DOF_Alias];
            for (int i = 0; i < system.DOF_Alias; ++i)
                alias[i] = q[i];
        }

    }
}
